#include "car_step_helper.hpp"

int main()
{
    if (car_init() != YJ_OK)
    {
        while (1)
        {
            LOG(ERROR) << "car init failed";
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        }
    }

    if (car_control_init() != YJ_OK)
    {
        while (1)
        {
            LOG(ERROR) << "car init failed";
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        }
    }

    double x_pos, y_pos, yaw_pos, x_zt_pos, y_zt_pos;
    int round = 0;
    int code_color[6] = {1, 2, 3, 3, 2, 1};
    int cjg_color[3] = {3, 2, 1};
    int zc_color[3] = {3, 2, 1};
    x_pos = 0;
    y_pos = 0;
    yaw_pos = 0;

    Pcar->set_speed(0.0f, 0.0f, 0.0f, 0);
    LOG(INFO) << "car stoped";
    std::this_thread::sleep_for(std::chrono::milliseconds(6000));
    reset_mission();

    set_car_x_speed_limit(500, -500);
    set_car_y_speed_limit(200, -200);
    set_claw_y_speed_limit(5000, -5000);
    set_car_yaw_speed_limit(180, -180);

    LOG(INFO) << "car inited";

    uint8_t temp;

    // while (Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_jgq0, temp) != YJ_OK)
    // {
    //     LOG(ERROR) << "get color not set";
    //     std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    // }

    // Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_jgq0, temp);
    // cjg_color[0] = temp;
    // Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_jgq1, temp);
    // cjg_color[1] = temp;
    // Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_jgq2, temp);
    // cjg_color[2] = temp;
    // Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_zcq0, temp);
    // zc_color[0] = temp;
    // Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_zcq1, temp);
    // zc_color[1] = temp;
    // Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_zcq2, temp);
    // zc_color[2] = temp;

    // LOG(INFO) << "cjg_color" << cjg_color[0] << cjg_color[1] << cjg_color[2];
    // LOG(INFO) << "zc_color" << zc_color[0] << zc_color[1] << zc_color[2];
    // std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    while (Pcar->read_start_flag() != true)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }
    hwt101_dev->reset();

    // while(1);

    std::thread t1(yaw_sensor_read_thread_entry);
    std::thread t2(xy_sensorless_read_thread_entry);
    std::thread t3(car_yaw_control_thread_entry);
    std::thread t4(car_all_control_thread_entry);
    std::thread t5(car_x_control_thread_entry);
    std::thread t6(car_y_control_thread_entry);
    std::thread t7(car_y_claw_control_thread_entry);
    std::thread t8(xy_sensor_read_thread_entry);

    x_pos += 125;
    y_pos += 115;
    yaw_pos += 0;
    set_car_move(x_pos, y_pos, yaw_pos); // 出库
    waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
    LOG(INFO) << "出库";

    x_pos += 600;
    y_pos += 100;
    yaw_pos += 0;
    set_car_move(x_pos, y_pos, yaw_pos); // 扫码
    waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);

    LOG(INFO) << "扫码";

    int scan_retry = 1;
    set_car_x_speed_limit(100, -100);
    while (Pcar->scan_code() != YJ_OK)
    {
        set_car_move(x_pos - scan_retry * 200, y_pos, yaw_pos); // 扫码
        waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
        LOG(INFO) << "re扫码";
        scan_retry *= -1;
    }
    set_car_x_speed_limit(500, -500);
    Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 100, 200, zdt_motor::motor_pos_mode_absolute);
    Pcar->set_bracket_pos(BRACKET_TABLE_RANEG, 500, 200, zdt_motor::motor_pos_mode_absolute, true);
    uint8_t color_temp[6];
    for (int round_t = 0; round_t < 2; round_t++)
        for (int i = 0; i < 3; i++)
        {
            Pcar->read_color_index_with_round(round_t, i, color_temp[i + round_t * 3]);
        }

    for (int i = 0; i < 6; i++)
    {
        code_color[i] = color_temp[i];
        LOG(INFO) << "code_color[" << i << "]=" << (int)code_color[i];
    }
    // ##########################################
    // ###############扫码动作#####################
    // ##########################################

    x_pos += 700;
    y_pos += -135;
    yaw_pos += 0;
    x_zt_pos = x_pos;
    y_zt_pos = y_pos;

    for (int i = 0; i < 2; i++)
    {
        set_car_move(x_pos, y_pos, yaw_pos); // 去转台前
        waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
        LOG(INFO) << "去转台前";

        // ##########################################
        // ###############抓取动作#####################
        // ##########################################

        //     0.5f,    // Kp
        // 0.01f,   // Ki
        // 0.2f,    // Kd
        set_car_x_speed_limit(200, -200);
        x_pid_in->set_pid_value(0.4f, 0.03f, 0.4f);
        LOG(INFO) << "准备抓";

        set_car_x_speed_limit(0, 0);
        double r_angle, claw_y;
        Pcar->set_claw_channel(1, 70);
        for (int j = 0; j < 3; j++)
        {
            Pcar->set_bracket_pos(BRACKET_TABLE_RANEG + 50, 500, 200, zdt_motor::motor_pos_mode_absolute);
            Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 100, 200, zdt_motor::motor_pos_mode_absolute, true);
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
            while (cam->is_move(code_color[j + round * 3]) != YJ_OK)
                ;
            if (start_trace_color_with_claw((camera::camera_color_en)(code_color[j + round * 3]), 20000, 10, true) != YJ_OK)
            {
                continue;
            }
            pick_obj(j, 1);
        }
        //         1.5f,    // Kp
        // 0.02f,   // Ki
        // 0.5f,    // Kd
        set_car_x_speed_limit(400, -400);
        x_pid_in->set_pid_value(1.5f, 0.02f, 0.5f);

        // std::this_thread::sleep_for(std::chrono::milliseconds(800));
        set_car_move(x_zt_pos, y_zt_pos, yaw_pos);
        waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
        Pcar->set_bracket_pos(BRACKET_TABLE_RANEG - 50, 500, 200, zdt_motor::motor_pos_mode_absolute);
        Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 100, 200, zdt_motor::motor_pos_mode_absolute);
        LOG(INFO) << "回转台前";

        x_pos += -400;
        y_pos += 35;
        yaw_pos += 0;
        set_car_move(x_pos, y_pos, yaw_pos); // 十字路口
        waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
        LOG(INFO) << "十字路口";

        double angle = cam->get_angle();
        LOG(INFO) << "直线检测前" << angle;
        // yaw_pos=adjust_angle(get_car_yaw_angle(),angle);
        angle = cam->get_angle();
        LOG(INFO) << "直线检测后" << angle;

        x_pos += 0;
        y_pos += 0;
        yaw_pos += -90;
        set_car_move(x_pos, y_pos, yaw_pos); // 十字路口
        waiting_for_move_finish(waiting_mode_car_yaw, 10000, 0.5);
        LOG(INFO) << "十字路口";

        angle = cam->get_angle();
        LOG(INFO) << "直线检测前" << angle;
        // yaw_pos=adjust_angle(get_car_yaw_angle(),angle);
        angle = cam->get_angle();
        LOG(INFO) << "直线检测后" << angle;

        x_pos -= 1720;
        y_pos += 0;
        yaw_pos += 0;
        set_car_move(x_pos, y_pos, yaw_pos); // 出去长直线
        waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
        LOG(INFO) << "出去长直线";
        Pcar->set_bracket_pos(BRACKET_TABLE_RANEG, 500, 200, zdt_motor::motor_pos_mode_absolute);

        x_pos -= 0;
        y_pos += 0;
        yaw_pos -= 90;
        set_car_move(x_pos, y_pos, yaw_pos); // 进入粗加工区
        waiting_for_move_finish(waiting_mode_car_yaw, 10000, 0.5);
        LOG(INFO) << "进入粗加工区";

        x_pos -= 0;
        y_pos -= 70;
        yaw_pos += 0;
        set_car_move(x_pos, y_pos, yaw_pos); // 进入粗加工区
        waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
        LOG(INFO) << "进入粗加工区";

        // ##########################################
        // ###############粗加工动作#####################
        // ##########################################
        double cjg_angles[3] = {0, 0, 0};
        double cjg_claw[3] = {0, 0, 0};
        x_pid_in->set_pid_value(0.4f, 0.03f, 0.4f);
        Pcar->set_bracket_pos(BRACKET_TABLE_RANEG, 500, 200, zdt_motor::motor_pos_mode_absolute);
        Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 100, 200, zdt_motor::motor_pos_mode_absolute);
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
        // if (round == 0)
        //     get_ground_color(cjg_color);
        LOG(INFO) << "粗加工区颜色" << cjg_color[0] << cjg_color[1] << cjg_color[2];
        start_trace_color(trace_mode_car_y, (camera::camera_color_en)cjg_color[1], waiting_dead_zone + 15);
        x_pos = get_car_x_mm();
        y_pos = get_car_y_mm();
        LOG(INFO) << "校准完成";
        set_car_x_speed_limit(0, 0);
        for (int k = 0; k < 3; k++)
        {
            if (code_color[k + 3 * round] == cjg_color[1])
            {
                stand_put(1, k, code_color[k + 3 * round], 0, cjg_angles[k], cjg_claw[k]);
            }
            else if (code_color[k + 3 * round] == cjg_color[0])
            {
                stand_put(0, k, code_color[k + 3 * round], 0, cjg_angles[k], cjg_claw[k]);
            }
            else
            {
                stand_put(2, k, code_color[k + 3 * round], 0, cjg_angles[k], cjg_claw[k]);
            }
        }
        for (int k = 0; k < 3; k++)
        {
            if (code_color[k + 3 * round] == cjg_color[1])
            {
                stand_pick(1, k, code_color[k + 3 * round], cjg_angles[k], cjg_claw[k]);
            }
            else if (code_color[k + 3 * round] == cjg_color[0])
            {
                stand_pick(0, k, code_color[k + 3 * round], cjg_angles[k], cjg_claw[k]);
            }
            else
            {
                stand_pick(2, k, code_color[k + 3 * round], cjg_angles[k], cjg_claw[k]);
            }
        }
        set_car_x_speed_limit(400, -400);
        x_pid_in->set_pid_value(1.5f, 0.02f, 0.5f);
        Pcar->set_bracket_pos(BRACKET_TABLE_RANEG, 500, 200, zdt_motor::motor_pos_mode_absolute);
        Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 100, 200, zdt_motor::motor_pos_mode_absolute);
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
        // ##########################################
        // ###############校准动作#####################
        // ##########################################
        angle = cam->get_angle();
        LOG(INFO) << "直线检测前" << angle;
        // yaw_pos=adjust_angle(get_car_yaw_angle(),angle);
        angle = cam->get_angle();
        LOG(INFO) << "直线检测后" << angle;

        x_pos -= 825;
        y_pos += 70;
        yaw_pos += 0;
        set_car_move(x_pos, y_pos, yaw_pos); // 前往暂存区
        waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
        LOG(INFO) << "前往暂存区";

        x_pos -= 0;
        y_pos += 0;
        yaw_pos -= 90;
        set_car_move(x_pos, y_pos, yaw_pos); // 前往暂存区
        waiting_for_move_finish(waiting_mode_car_yaw, 10000, 0.5);
        LOG(INFO) << "前往暂存区";

        x_pos -= 825;
        y_pos -= 70;
        yaw_pos -= 0;
        set_car_move(x_pos, y_pos, yaw_pos); // 前往暂存区
        waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
        LOG(INFO) << "前往暂存区";

        // ##########################################
        // ###############暂存动作#####################
        // ##########################################
        x_pid_in->set_pid_value(0.4f, 0.03f, 0.4f);
        Pcar->set_bracket_pos(BRACKET_TABLE_RANEG, 500, 200, zdt_motor::motor_pos_mode_absolute);
        Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 100, 200, zdt_motor::motor_pos_mode_absolute);
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
        // if (round == 0)
        //     get_ground_color(zc_color);
        start_trace_color(trace_mode_car_y, (camera::camera_color_en)zc_color[1], waiting_dead_zone + 15);
        x_pos = get_car_x_mm();
        y_pos = get_car_y_mm();
        LOG(INFO) << "校准完成";
        set_car_x_speed_limit(0, 0);
        for (int k = 0; k < 3; k++)
        {
            if (code_color[k + 3 * round] == zc_color[1])
            {
                stand_put(1, k, code_color[k + 3 * round], round, cjg_angles[k], cjg_claw[k]);
            }
            else if (code_color[k + 3 * round] == zc_color[0])
            {
                stand_put(0, k, code_color[k + 3 * round], round, cjg_angles[k], cjg_claw[k]);
            }
            else
            {
                stand_put(2, k, code_color[k + 3 * round], round, cjg_angles[k], cjg_claw[k]);
            }
        }
        x_pid_in->set_pid_value(1.5f, 0.02f, 0.5f);
        set_car_x_speed_limit(400, -400);
        Pcar->set_bracket_pos(BRACKET_TABLE_RANEG, 500, 200, zdt_motor::motor_pos_mode_absolute);
        Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 100, 200, zdt_motor::motor_pos_mode_absolute);
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
        // ##########################################
        // ###############校准动作#####################
        // ##########################################

        x_pos -= 895;
        y_pos += 70;
        yaw_pos += 0;
        set_car_move(x_pos, y_pos, yaw_pos); // 回来长直线
        waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
        LOG(INFO) << "回来长直线";

        x_pos -= 0;
        y_pos += 0;
        yaw_pos -= 90;
        set_car_move(x_pos, y_pos, yaw_pos); // 回来长直线
        waiting_for_move_finish(waiting_mode_car_yaw, 10000, 0.5);
        LOG(INFO) << "回来长直线";

        x_pos -= 300;
        y_pos -= 85;
        yaw_pos += 0;
        x_zt_pos = x_pos;
        y_zt_pos = y_pos;

        Pcar->set_bracket_pos(BRACKET_TABLE_RANEG, 500, 200, zdt_motor::motor_pos_mode_absolute);
        x_pos -= 125;
        round++;
    }
    y_pos += 85;
    set_car_move(x_pos, y_pos, yaw_pos); // 回到转盘
    waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
    LOG(INFO) << "回到转盘";

    x_pos -= 800;
    x_pos -= 500;
    y_pos -= 85;
    yaw_pos += 0;
    set_car_move(x_pos, y_pos, yaw_pos); // 回出生点长直线
    waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
    LOG(INFO) << "回出生点长直线";

    x_pos -= 125;
    y_pos -= 115;
    yaw_pos += 0;
    set_car_move(x_pos, y_pos, yaw_pos); // 回出生点长直线
    waiting_for_move_finish(waiting_mode_car_x_y_yaw, 10000, waiting_dead_zone);
    LOG(INFO) << "回出生点长直线";

    // ##########################################
    // ###############完赛#####################
    // ##########################################
    LOG(INFO) << "完赛";

    while (1)
    {

        std::this_thread::sleep_for(std::chrono::milliseconds(5000));
    }
}
